#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <arpa/inet.h>
#include <unistd.h>

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
 
int main(void)
{
	int ret = -1;
        std::vector<uchar> data_encode;
        std::vector<int> quality;

        //Mat image=imread("test1.png");
        //Mat image=imread("1.jpg");
        Mat image=imread("1234.jpeg");
        //imshow("test",image);
        //waitKey(0);   //这步不能漏掉掉，否则显示图片的窗口不会出来。
        //参考自：https://blog.csdn.net/qq_37406130/article/details/78820176
        imencode(".jpg", image, data_encode,quality);//将图像编码
	//char encodeImg[65535];
        //char encodeImg[60000];
        //char encodeImg[300000];
        char encodeImg[400000];  //改成400000就是想看看实际存储图像的数组长度不等于图像实际长度，比图像实际长度大的时候。
        int nSize = data_encode.size();
		for (int i = 0; i < nSize; i++)
		{
			encodeImg[i] = data_encode[i];   //这样就把图像的数据放到char数组里面，然后让UDP传输
		}
	// 1、使用socket()函数获取一个socket文件描述符
	int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
	if (-1 == sockfd)
	{
		printf("socket open err.");
		return -1;
	}
 
	// 2. 准备接收方的地址和端口，'192.168.0.107'表示目的ip地址，8266表示目的端口号 
    struct sockaddr_in sock_addr = {0};	
	sock_addr.sin_family = AF_INET;                         // 设置地址族为IPv4
	sock_addr.sin_port = htons(8266);						// 设置地址的端口号信息
	//sock_addr.sin_addr.s_addr = inet_addr("192.168.0.107");	//　设置IP地址
        sock_addr.sin_addr.s_addr = htonl(INADDR_ANY);
 
    // 3. 发送数据到指定的ip和端口
	char sendbuf[]={"hello world, I am UDP."};
    int cnt = 10;
   // while(cnt--)
    {
        //ret = sendto(sockfd, sendbuf, sizeof(sendbuf), 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr));
        //ret = sendto(sockfd, encodeImg, sizeof(encodeImg), 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr));  //发送图片的信息
    
 
       //int imgFlag=11111;
       //int imgLen = 300000  ;
       //int imgLen = 299999  ;
       //int imgPackUnit = 60000;
       //int imgpackNum = imgLen/imgPackUnit+1;
       //int imgtimestampsec = 1111111111;
       //int imgtimestampnsec = 222222222;



     int imuFlag=101010;
     int imgtimestampsec = 1111111111; 
     int imgtimestampnsec = 222222222; 
     double orientation_x=0.2;
     double orientation_y=0.2;
     double orientation_z=0.2;
     double orientation_w=0.2;
     double angular_velocity_x=0.2;
     double angular_velocity_y=0.2;
     double angular_velocity_z=0.2;
     double linear_acceleration_x=0.2;
     double linear_acceleration_y=0.2;
     double linear_acceleration_z=0.2;

     double orientation_covariance_[9]={-1.0,0,0,0,0,0,0,0};
     double angular_velocity_covariance_[9]={0.01,0,0,0,0.01,0,0,0,0.01};
     double linear_acceleration_covariance_[9]={0.01,0,0,0,0.01,0,0,0,0.01};

     
     char imu_data[92];

     memcpy(imu_data,&imuFlag,4);
     memcpy(imu_data+4,&imgtimestampsec,4);
     memcpy(imu_data+8,&imgtimestampnsec,4);
     memcpy(imu_data+12,&orientation_x,8);
     memcpy(imu_data+20,&orientation_y,8);
     memcpy(imu_data+28,&orientation_z,8);
     memcpy(imu_data+36,&orientation_w,8);
     memcpy(imu_data+44,&angular_velocity_x,8);
     memcpy(imu_data+52,&angular_velocity_y,8);
     memcpy(imu_data+60,&angular_velocity_z,8);
     memcpy(imu_data+68,&linear_acceleration_x,8);
     memcpy(imu_data+76,&linear_acceleration_y,8);
     memcpy(imu_data+84,&linear_acceleration_z,8);
     
     

       //char img_head_char[4];
       //char img_head_char[16];
       //memcpy(img_head_char,&imgFlag,4);
       //memcpy(img_head_char+4,&imgLen,4);
       //memcpy(img_head_char+8,&imgPackUnit,4);
       //memcpy(img_head_char+12,&imgpackNum,4);
       //memcpy(img_head_char+16,&imgtimestampsec,4);
       //memcpy(img_head_char+20,&imgtimestampnsec,4);
       //ret = sendto(sockfd, img_head_char, 4 , 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr)); 
       //ret = sendto(sockfd, img_head_char, 24 , 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr));
       
       ret = sendto(sockfd, imu_data, 92 , 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr));
 
       /******************************************************************
        int i1=0;
       // for(i1=0;i1<5;i1++)
       for(i1=0;i1<imgpackNum;i1++)
          {
             ret = sendto(sockfd, encodeImg + i1*60000, 60000, 0, (struct sockaddr*)&sock_addr, sizeof(sock_addr)); 
          }
       ********************************************************************/
	    printf("ret = %d \n",ret);
       // sleep(1);
    }
 
    // 4. 关闭套接字
    close(sockfd);
}
 

